#include "Torus.h"
#include <iostream>
using namespace std;

Vector Torus::getNormal (const Point& p) const
{
	Point locP;
	double k, nx, ny, nz;

	locP = globalToLocal * p;

	k = 4 * (locP.get_x() * locP.get_x() + locP.get_y() * locP.get_y() + locP.get_z() * locP.get_z()
		+ bigRadius * bigRadius - smallRadius * smallRadius);
	
	nx = locP.get_x() * k - 8 * bigRadius * bigRadius * locP.get_x();
	ny = locP.get_y() * k;
	nz = locP.get_z() * k - 8 * bigRadius * bigRadius * locP.get_z();
	
	Vector res = Vector(nx, ny, nz);
	res.normalize();
	res = localToGlobal * res;
	return res;
}

bool Torus::intersection(Ray& ray, Intersection& i) const
{
	if (!boundingBox.intersection(ray,i))
		return false;
	
	Point O = globalToLocal * ray.getOrigin();
	Vector D = globalToLocal * ray.getDirection();

	Point intersection;
	Vector normal;
	double a, b, c, d, e, t;
	Polynomial solver;
	double* roots;
	int nbRoots;
	
	double tmpA = D.get_x() * D.get_x() + D.get_y() * D.get_y() + D.get_z() * D.get_z();
	double tmpB = 2.0 * (D.get_x() * O.get_x() + D.get_y() * O.get_y() + D.get_z() * O.get_z());
	double tmpC = O.get_x() * O.get_x() + O.get_y() * O.get_y() + O.get_z() * O.get_z() + bigRadius * bigRadius - smallRadius * smallRadius;
	
	double tmp = 4.0 * bigRadius * bigRadius;
	double tmpD = tmp * (D.get_x() * D.get_x() + D.get_z() * D.get_z());
	double tmpE = tmp * 2.0 * (D.get_x() * O.get_x() + D.get_z() * O.get_z());
	double tmpF = tmp * (O.get_x() * O.get_x() + O.get_z() * O.get_z());
	
	a = tmpA * tmpA;
	b = 2.0 * tmpA * tmpB;
	c = 2.0 * tmpA * tmpC + tmpB * tmpB - tmpD;
	d = 2.0 * tmpB * tmpC - tmpE;
	e = tmpC * tmpC - tmpF;

	roots = new double[4];
	solver = Polynomial(a, b, c, d, e);	
	nbRoots = solver.SolveQuartic(roots);
	t = DBL_MAX;
	
	for (int j = 0; j < nbRoots; j++) {
		if (roots[j] > 0.001 && roots[j] < t)
			t = roots[j];
	}
	
	delete(roots);
	
	if (t != DBL_MAX)
	{
		intersection = localToGlobal * Point(O.get_x()+D.get_x()*t, O.get_y()+D.get_y()*t, O.get_z()+D.get_z()*t);

		i.setC(color);
		i.setP(intersection);
		i.setNormal(getNormal(intersection));
		i.setDistance(t);

        	return(true);
	}
	return(false);
}

Torus::Torus(const Point &p, const double &r1, const double &r2, const double &ax, const double &az)
{
	initialize(Vector(Point(0,0,0),p), ax, az);
	location=p;
	bigRadius=r1;
	smallRadius=r2;
	boundingBox = Box(p, (r1+r2)*2, r2*2, (r1+r2)*2, ax, az);
	//boundingBox = Box(p, (r1+r2)*2, (r1+r2)*2, (r1+r2)*2, 0, 0);
}
